#include <Wire.h>
#include<String.h>
//#include <Servo.h>

#define PinA 2 //外部中断0
#define PinB 3
#define PinAs 12//编码器的OUTB信号连接到数字端口8
#define PinBs 13
#define A 4  //车A轮
#define B 6  //车B轮

// Servo myservo;  // create servo object to control a servo
int servoPos = 0, changeCount = 0;
//接收的数据
String reciverData = "";
//要发送的数据
String sendData;
char * sendDatap;
int da = 5, da1 = 4, da2 = 7;     // the PWM pin the LED is attached to
int db = 6, db1 = 8, db2 = 9;
unsigned long time1 = 0, time2 = 0; // 时间标记
long count1 = 0, count2 = 0;
long oldcount1 = 0, oldcount2 = 0;

double v1 = 0, v2 = 0; //左右轮的速度 mm/s
double targetv1 = 0 , targetv2 = 0; //目标速度 mm/s
int pwma = 0, pwmb = 0;

void setup() {
  Wire.setClock(400000);
  Wire.begin(3);                // join i2c bus with address #8h
  Wire.onReceive(receiveEvent); // register event
  Wire.onRequest(requestEvent); // register event
  pinMode(da, OUTPUT);
  pinMode(da1, OUTPUT);
  pinMode(da2, OUTPUT);
  pinMode(db, OUTPUT);
  pinMode(db1, OUTPUT);
  pinMode(db2, OUTPUT);
  attachInterrupt(0, Code1, FALLING);//脉冲中断函数
  attachInterrupt(1, Code2, FALLING);//脉冲中断函数
  Serial.begin (250000);
  digitalWrite(da1, LOW);
  digitalWrite(da2, LOW);
  digitalWrite(db1, LOW);
  digitalWrite(db2, LOW);

  TCCR1A = 0;
  TCCR1B = 1 << WGM13 | 1 << WGM12 | 1 << CS11 | 1 << CS10;
  ICR1 = 2500;
  bitSet(TIMSK1, ICIE1); // 允许输入捕捉中断
}

/*********************************************
函数功能：速度控制
           8(正)
    4(A)   5 (Stop)   6(B)
           2(反)
*********************************************/

void controlv(int which, int v) {
  v = XianfuPwm(v);
  if (which == A) {
    if (v >= 0) { //8 是正转  2是倒转
      analogWrite(da, v);
      digitalWrite(da1, HIGH);
      digitalWrite(da2, LOW);
    } else{
      analogWrite(da, myabs(v));
      digitalWrite(da1, LOW);
      digitalWrite(da2, HIGH);
    }
  } else if (which == B) {
    if (v>=0) { //8 是正转  2是倒转
      analogWrite(db, v);
      digitalWrite(db1, HIGH);
      digitalWrite(db2, LOW);
    } else{
      analogWrite(db, myabs(v));
      digitalWrite(db1, LOW);
      digitalWrite(db2, HIGH);
    }
  }
}

/*
    计算CRC8校验结果
    多项式：x8+x2+x1+1   0x07（0x107）

*/
char calcCRC8(char *ptr, char len)
{
  char i;
  char crc = 0x00;

  while (len--)
  {
    crc ^= *(ptr++);
    for (i = 8; i > 0; --i)
    {
      if (crc & 0x80)
        crc = (crc << 1) ^ 0x07;
      else
        crc = (crc << 1);
    }
  }
  return (crc);
}

/********************************************************
函数功能： 分析接收到的数据
入口参数：无
返回值：无
********************************************************/
void analyseReciverData() {
  if (reciverData.length() >= 2) {
    int len = reciverData.length() + 1, flag = 0;
    //处理收到的数据
    char reciverChar[len] = "", c;
    for(int i = 0 ;i < len-1;i++){
      reciverChar[i]=reciverData.charAt(i);
      Serial.print(byte(reciverData.charAt(i)));
    }
    Serial.print(reciverChar);
    byte cr8 = byte(calcCRC8(reciverChar, len - 2));
    if ((cr8 == byte(reciverData.charAt(len-2)))&&cr8) {
      Serial.println("校验成功！  ");
      int reciverLength = reciverData.length();
      String WheelA = "", WheelB = "" , servoString = "";
      for (int i = 2; i < len - 2; i++) {
        c = reciverData.charAt(i);
        if (c == '|') {
          flag++;
          continue;
        }
        if (flag == 0) {
          WheelA += c;
        } else if (flag == 1) {
          WheelB += c;
        } else if (flag == 2) {
          servoString += c;
        }
      }
      targetv1 = WheelA.toInt();
      targetv2 = WheelB.toInt();
      int servo  = servoString.toInt();
      controlv(A, pwma/5);
      controlv(B, pwmb/5);
    } else {
      Serial.println("校验失败！");
    }
  }
}


void loop() {
  while (1) {
    //数据放到这里处理然后发送
    String str = "#";
    str += v1;
    str += "|";
    str += v2;
    str += "#";
    int len = str.length() + 1;
    char c[len];
    str.toCharArray(c, len);
    c[len - 1] = calcCRC8(c, len - 1);
    sendDatap = c;
    //分析接收到的数据
    analyseReciverData();
    Serial.print(v1);
    Serial.print("   ");
    Serial.println(v2);
    Serial.print(targetv1);
    Serial.print("   ");
    Serial.println(targetv2);
    Serial.print(pwma);
    Serial.print("   ");
    Serial.println(pwmb);
  }
}
/********************************************
函数功能：接收数据
********************************************/
void receiveEvent(int howMany) {
  Serial.print("reciverEnent");
  // if(Wire.available()>3){
    reciverData="";
    while (0 < Wire.available())
    reciverData += char(Wire.read()); // receive byte as a character
  // }

}


void requestEvent() {
  Wire.write(sendDatap); // respond with message of 6 bytes
}


/**************************************************************************
函数功能：增量PI控制器
入口参数：编码器测量值，目标速度
返回  值：电机PWM
根据增量式离散PID公式 
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)代表本次偏差 
e(k-1)代表上一次的偏差  以此类推 
pwm代表增量输出
在我们的速度控制闭环系统里面，只使用PI控制
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)
**************************************************************************/
float Kp=1,Ki=0.5; 
int PIA(int Encoder,int Target)
{   
   static int Bias,Pwm,Last_bias;
   Bias=Target-Encoder;                //计算偏差
   Pwm+=Kp*(Bias-Last_bias)+Ki*Bias;   //增量式PI控制器
   Last_bias=Bias;                     //保存上一次偏差 
   Pwm = XianfuPwm(Pwm);
   return Pwm;                         //增量输出
}
int PIB(int Encoder,int Target)
{   
   static int Bias,Pwm,Last_bias;
   Bias=Target-Encoder;                //计算偏差
   Pwm+=Kp*(Bias-Last_bias)+Ki*Bias;   //增量式PI控制器
   Last_bias=Bias;                     //保存上一次偏差 
    Pwm = XianfuPwm(Pwm);
   return Pwm;                         //增量输出
}



extern"C"{
  //测速单元
ISR(TIMER1_CAPT_vect) {
  // Serial.print("creatygiuh");
  //测速
  double detv1 = count1 - oldcount1;
  double detv2 = count2 - oldcount2;
  oldcount1 = count1;
  oldcount2 = count2;
  detv1 *= 0.51554341;
  detv2 *= 0.51554341;
  //速度更新完成
  v1 = detv1 * 10;  //mm/ms   mm/s   m/min
  v2 = detv2 * 10; //mm/ms   mm/s

  //PI
  pwma = PIA(v1,targetv1);
  pwmb = PIB(v2,targetv2);


}
}
/**************************************************************************
函数功能：限制PWM赋值 
入口参数：无
返回  值：无
**************************************************************************/
int  XianfuPwm(int a)
{ 
    int Amplitude=1200;    //===PWM满幅是
    if(a<-Amplitude) a=-Amplitude;  
    if(a>Amplitude)  a=Amplitude; 
    return a;
}

/**************************************************************************
函数功能：绝对值函数
入口参数：int
返回  值：unsigned int
**************************************************************************/
int myabs(int a)
{        
    int temp;
    if(a<0)  temp=-a;  
    else temp=a;
    return temp;
}

// 编码器2计数中断子程序
void Code1()
{
  if ((millis() - time1) > 5)
    if ((digitalRead(PinA) == LOW) && (digitalRead(PinAs) == HIGH))
      count1--;
    else
      count1++;
  time1 == millis();
}
// 编码器2计数中断子程序
void Code2()
{
  if ((millis() - time2) > 5)
    if ((digitalRead(PinB) == LOW) && (digitalRead(PinBs) == HIGH))
      count2--;
    else
      count2++;
  time1 == millis();
}
